package org.nashua.tt151;

import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SimpleRobot;
import org.nashua.tt151.RobotMap.DualAction;
import org.nashua.tt151.RobotMap.Victor.Power;

/**
 * This class is sample code for driving the robot using four Victor and the
 * Joystick class that is being read from a Logitech DualAction F310 (D mode).
 *
 * NOTE: This is using FRCSquawkSDK Version 2012.3077. When the 2013 versions
 * are released this class will be updated according.
 */
public class DriveVictorsByJoystick extends SimpleRobot {
    /* Hardware */
    public static Victor backLeft = new Victor(RobotMap.Victor.BACK_LEFT);
    public static Victor backRight = new Victor(RobotMap.Victor.BACK_RIGHT);
    public static Victor frontLeft = new Victor(RobotMap.Victor.FRONT_LEFT);
    public static Victor frontRight = new Victor(RobotMap.Victor.FRONT_RIGHT);
    public static Joystick joy = new Joystick(1);  // 1st Joystick Detected

    /* Other Variables */
    public static double power = Power.DEFAULT_POWER;  // Set to default power

    /**
     * This method is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        while (isEnabled() && isOperatorControl()) {
            /* Determine Power */
            power = Power.DEFAULT_POWER; // Reset to default power
            if (joy.getRawButton(DualAction.Button.LEFT_TRIGGER))
                power = Power.CREEP_POWER; // Set to creep while holding LT
            if (joy.getRawButton(DualAction.Button.RIGHT_TRIGGER))
                power = Power.MAX_POWER;  // Set to max power while holding RT

            /*
             * Store axis positions to avoid accidentally setting the back and
             * the front Victors to 2 different values. The stored values are
             * multiplied by the power multiplier.
             *
             * Victors require an int 0 to 255 not a double -1 to 1 and we use
             * the setRaw method
             * instead of set
             */
            // For DualAction this is the same as: double left = joy.getY();
            double left = joy.getRawAxis(DualAction.Axis.LEFT_Y) * power;
            // To make a double an int we need to cast (int)(double-to-cast)
            // Casting uses the form (new-type-or-class)(value-to-cast)
            int leftRaw = (int)((left+1)*255);
            /*
             * Generally, the value of the right y axis needs to be reversed
             * since the motors are rotating in the opposite direction of the
             * left motors. Always check the robot as this is not always true.
             */
            double right = -joy.getRawAxis(DualAction.Axis.RIGHT_Y) * power;
            int rightRaw = (int)((right+1*255));
            backLeft.setRaw(leftRaw);
            frontLeft.setRaw(leftRaw);
            backRight.setRaw(rightRaw);
            frontRight.setRaw(rightRaw);
        }
    }
}
